/**
  MOMD project, Anyang Normal University, IMP-CAS
  \file ETFMath.h
  \class ETFMath
  \brief Math class, to provide some general math methods. Note that this is a
  tool class, so it is defined to be a static class.
  \author SUN Yazhou, aisa.rabbit@163.com
  \date Created: 2020/07/09
  \date Last modified: 2020/10/04 by SUN Yazhou
  \copyright 2020 SUN Yazhou
  \copyright MOMD project, Anyang Normal University, IMP-CAS
*/

#include <cmath>
#include <iostream>
#include <cstring>
#include <algorithm>
#include <utility>
#include <catch.hpp>
#include <array>
#include "ETFMath.h"
#include "ETFMsg.h"
#include "ETFDetectorManager.h"
#include "ETFTrack.h"
#include "ETFTrackTa.h"
#include "ETFDCHit.h"

using std::swap;
using std::max;
using std::cout;
using std::endl;
using std::array;

ClassImp(ETFMath)

// #define DEBUG

// the z border of the effective magField (in mm)
const double ETFMath::kzMagIn = -555.5;
const double ETFMath::kzMagOut = 555.5;

// r_global = R.r_local
// angle0: yaw, angle1: pitch, angle2: roll, intrinsic rotation; (y-x'-z")
// R(yaw, pitch, roll) = Ry(yaw).Rx'(pitch).Rz"(roll)
void ETFMath::rotate(const double *pIn, double *pOut, const double *angIn){
	double s1 = sin(angIn[0]), s2 = sin(angIn[1]), s3 = sin(angIn[2]);
	double c1 = cos(angIn[0]), c2 = cos(angIn[1]), c3 = cos(angIn[2]);

	pOut[0] = (c1*c3+s1*s2*s3)* pIn[0] +(c3*s1*s2-c1*s3)* pIn[1] +c2*s1* pIn[2];
	pOut[1] = c2*s3*            pIn[0] +c2*c3*            pIn[1] -s2*    pIn[2];
	pOut[2] = (c1*s2*s3-c3*s1)* pIn[0] +(c1*c3*s2+s1*s3)* pIn[1] +c1*c2* pIn[2];
} // end member function rotate

/// returns the value ln[Gamma(xx)] for xx > 0.
double ETFMath::gammaln(double xx){
  if(xx <= 0.) ETFMsg::Error("ETFMath", "gammaln: x > 0. is mandatory.");
  // internal arithmetic will be done in double precision, a nicety that you can
  // omit if five-figure accuracy is good enough
  static const double c[6] = {76.18009172947146,-86.50532032941677,
    24.01409824083091,-1.231739572450155,
    0.1208650973866179e-2,-0.5395239384953e-5};
  double tmp = xx + 5.5, x = xx; // (z+gamma+1/2), gamma = 5, N = 6
  tmp -= (x+0.5)*log(tmp);
  double ser = 1.000000000190015; // the additive series
  for(int i = 0; i < 6; i++) ser += c[i]/++x;
  return -tmp+log(2.5066282746310005*ser/xx); // sqrt(2pi)
} // end member function gammaln

// returns ln(n!)
double ETFMath::factln(int n){
  static double a[101]; // a static array is automatically initialized to zero

  if(n < 0) ETFMsg::Error("ETFMath", "factln: n is negative.");
  if(n <= 1) return 0.;
  if(n <= 100) return a[n] ? a[n] : (a[n]=gammaln(n+1.));
  return gammaln(n+1.); // out of range of table
} // end member function factln

/// \retval n!
int ETFMath::Factorial(int n){
  static int ntop = 4;
  static int a[33] = {1, 1, 2, 6, 24}; // fill in table only as required

  if(n < 0) ETFMsg::Error("ETFMath", "Factorial: n: %d is minus", n);
  // larger value than size of the table is required. Actually, this big a value
  // is going to overflow on many computers, but no harm in trying
  if(n > 32) return exp(gammaln(n+1.));
  while(ntop < n){
    ntop++;
    a[ntop] = a[ntop-1]*ntop;
  }
  return floor(0.5+a[n]); // clear off the roundoff error
} // end member function Factorial

// returns Binomial coefficients
int ETFMath::Binomial(int n, int m){
  if(m > n)
    ETFMsg::Error("ETFMath",
      "Binomial: n: %d is larger than m: %d!", n, m);

  // the floor function cleans up roundoff error for smaller values of n and m
  return floor(0.5+exp(factln(n)-factln(m)-factln(n-m)));
} // end member function Binomial

double ETFMath::Beta(double z, double w){
  if(z < 0. || w < 0.) ETFMsg::Error("ETFMath", "Beta: z or z is minus");
  return exp(gammaln(z)+gammaln(w)-gammaln(z+w));
} // end member function Beta

/// \retval n!!
int ETFMath::BiFactorial(int n){
  return n <= 1 ? 1 : n * BiFactorial(n-2);
} // end member function BiFactorial

/// the incomplete gamma function gammap, gammap(a,x)=P(a,x)=gamma(a,x)/Gamma(a)
double ETFMath::gammap(double a, double x){
  if(x < 0. || a <= 0.) ETFMsg::Error("ETFMath", "gammap: invalid arguements");

  if(x < (a+1.)) return gser(a, x); // series representation
  return 1. - gcf(a, x);
} // end member function gammap
/// the incomplete gamma function gammaq, gammaq(a,x)=Q(a,x)=1-P(a,x)
double ETFMath::gammaq(double a, double x){
  if(x < 0. || a <= 0.) ETFMsg::Error("ETFMath", "gammaq: invalid arguements");

  if(x < (a+1.)) return 1. - gser(a, x); // continued fraction representation
  return gcf(a, x);
} // end member function gammaq

TEST_CASE("Incomplete Gamma function", "[gammap]"){
  // CHECK(ETFMath::gammap(1., 0.5) == Approx(0.606531).epsilon(1.e-8));
  // CHECK(ETFMath::gammaq(1., 0.5) == Approx(1.-0.606531).epsilon(1.e-8));
  CHECK(ETFMath::gammap(1., 0.5) == Approx(0.3934693402873666).epsilon(1.e-8));
  CHECK(ETFMath::gammap(1., 0.1) == Approx(0.09516258196404048).epsilon(1.e-8));
  CHECK(ETFMath::gammaq(1., 3.5) == Approx(0.0301973834223185).epsilon(1.e-8));
  CHECK(ETFMath::gammaq(1., 4.1) == Approx(0.016572675401761255).epsilon(1.e-8));
  CHECK(exp(ETFMath::gammaln(0.5)) == Approx(sqrt(ETFMath::Pi())).epsilon(1.e-8));
} // end TEST_CASE

// returns the incomplete gamma function P(a,x) evaluated by its series
// representation. Optionally returns ln[Gamma(a)] as gln.
double ETFMath::gser(double a, double x, double *gln){
  static const double ITMAX = 100; // maximum iteration times
  static const double EPS = 3.e-7; // fractional accuracy

  if(x < 0.) ETFMsg::Error("ETFMath", "gser: x < 0., x: %f", x);
  double ap = a, sum, item = sum = 1./a;
  for(int i = 0; i < ITMAX; i++){
    sum += (item *= x/++ap);
    if(fabs(item) < fabs(sum)*EPS)
      return sum * exp(-x + a*log(x) - (gln ? *gln=gammaln(a) : gammaln(a)));
  } // end for over i
  ETFMsg::Error("ETFMath", "gser: a too large, and IMAX too small.");
  return 0.; // never gets here
} // end member function gamser

// returns the incomplete gamma function Q(a,x) evaluated by its continued
// fraction representation. Optionally returns ln[Gamma(a)] as gln.
double ETFMath::gcf(double a, double x, double *gln){
  static const double ITMAX = 100; // maximum iteration times
  static const double EPS = 3.e-7; // factional accuracy
  // number near the smallest representable floating-point number
  static const double FPMIN = 1.e-30; // modified Lentz's algorithm

  // set up for evaluating continued fraction by modified Lentz's method with b0=0.
  // b1 = x+1.-a, c1 = \infty (since c0 = 0), d1 = 1/b1, fr1=b0+a1/b1=a1/b1=1/b1=d1
  double b = x+1.-a, c = 1./FPMIN, d = 1./b, fr = d;
  for(int i = 1; i < ITMAX; i++){
    double aip = -i*(i-a); // a_{i+1}
    b += 2.; d = aip*d+b; // b2, d2 = 1/(a2*d1+b2)
    if(fabs(d) < FPMIN) d = FPMIN;
    c = b+aip/c; // c2 = b2 + a2/c1
    if(fabs(c) < FPMIN) c = FPMIN;
    d = 1./d;
    double delta = c*d;
    fr *= delta; // fr_2 = fr_1*c2*d2
    if(fabs(delta-1.) < EPS)
      return exp(-x+a*log(x)-(gln ? *gln = gammaln(a) : gammaln(a)))*fr; // put factors in front
  } // end for over i
  ETFMsg::Error("ETFMath", "gcf: a too large, and IMAX too small.");
  return 0.; // never gets here
} // end member function gcf

/// fit the track and assign slope and intercept
double ETFMath::IterFit(const int np, const double *z, const double *x,
		const double *r, double &kL, double &bL){
	const int n = pow(2, np);
	double d2, kl, bl, db, dk, q = 0., d2min; // q: goodness of fit
	// dx not unkown, so set to minus deliberately
	vector<double> dx; dx.resize(np); dx.assign(np, -1.);
	LinearFit(z, x, dx.data(), np, bL, kL, db, dk, d2min, q);
	vector<double> zt, xt;
	zt.resize(np); xt.resize(np);
	zt.assign(np, -9999.); xt.assign(np, -9999.);
	const double cosTheta = 1./ sqrt(1.+kL*kL), sinTheta = kL*cosTheta;
	for(int i = n; i--;){
		for(int j = 0; j < np; j++){
			const double R = (2*((i>>j) & 0x1)-1) * r[j];
			zt[j] = z[j] + R*sinTheta;
			xt[j] = x[j] - R*cosTheta;
		} // end for over j
		LinearFit(zt.data(), xt.data(), dx.data(), np, bl, kl, db, dk, d2, q);
		if(d2 < d2min){ kL = kl; bL = bl; d2min = d2; }
	} // end for over i

	return d2min;
} // end member function IterFit

// iterative fit for united tracking of preTaTrk and postTaTrk. For X1-X2 bothly
// fired cases, their signs of drift distances are determined so that the track goes
// in between, and only the drift distance signs of the singly fired hits are iterated
// hence the name of IterFit4Single
// l: k3x+b3: postMagTrk
double ETFMath::IterFit4Single(ETFTrackTa *t0, ETFTrackTa *t1, const double k3,
		const double b3, double &fDxTa, double &fDx2){
	const int nh[2] = {t0->GetNofHits(), t1->GetNofHits()};
	int nSingle[2]{}; // number of singly fired layers
	for(int i = 2; i--;) if(3 == nh[i]) nSingle[i] = 1;
	const int n = pow(2, nSingle[0]+nSingle[1]); // count of iterations
	double kl[3]{}, bl[3]{}, d2[2]{};
	kl[2] = k3; bl[2] = b3; // postMagTrk's parameters
	double d2Tmin = 1.e200, d2T; // T: total
	double dX[2]; // [0-1]: dx2-dxTa

	// get the positions of all the hits //
	ETFTrackTa *trk[2] = {t0, t1};
  vector<double> z[2], x[2], r[2]; // [0-1]: [preTaTrk, postTaTrk]
  for(int I = 0; I < 2; I++){ // loop over preTaTrk and postTaTrk
    z[I].resize(nh[I]); x[I].resize(nh[I]); r[I].resize(nh[I]);
		int i = 0;
    for(auto &p : trk[I]->GetHits()){
			p.second->GetProjZXR(z[I][i], x[I][i], r[I][i]);
			i++;
		} // end for
  } // end for over I
	// zt, xt: wirePos+r
	vector<double> zt[2], xt[2], dx[2]; // dx: the sigma of x
	for(int i = 2; i--;){
		zt[i].resize(nh[i]); zt[i].assign(zt[i].size(), -9999.);
		xt[i].resize(nh[i]); xt[i].assign(xt[i].size(), -9999.);
		dx[i].resize(nh[i]); dx[i].assign(dx[i].size(), -1.); // minus -> sigma not known
	} // end for over i
	double db, dk, q = 0.; // temporary variables
	for(int i = 2; i--;){
		LinearFit(z[i].data(), x[i].data(), dx[i].data(), nh[i], bl[i], kl[i], db, dk, d2[i], q);
		trk[i]->SetD2(d2[i]);
	} // end if
	double cosTheta[2], sinTheta[2];
	for(int i = 0; i < 2; i++){
		cosTheta[i] = 1. / sqrt(1.+kl[i]*kl[i]);
		sinTheta[i] = kl[i] * cosTheta[i];
	} // end for over i
	// assign signs for X1-X2 bothly fired wires //
	int singleLayer[2] = {-1, -1}; // ids (vector subscripts) of the single layers
	for(int I = 0; I < 2; I++){ // loop over preTaTrk-postTaTrk
		for(int i = 0; i < 2; i++){ // loop over DCTa0-1
			const int ss = I*4 + i*2;
			if(!(*trk[I])[ss] || !(*trk[I])[ss+1]){ // find singly fired wires
				// singleLayer is 2 if the second pdc in the pdc array has a singleLayer
				// because the first 2 layers are bothly fired, i.e., x[I][singleLayer] starts
				// at singleLayer = 2
				singleLayer[I] = i ? 2 : 0; // singly fired
				continue; // singly fired PDC, no need for X1-X2 sign assignment
			} // end if
			//-- assign signs for bothly fired wires --//
			// find the subscripts offset (s) for the bothly fired wires //
			// Let's think: i=0 => s=0
			// i=1: singleLayer[I]=0 => s=1;  single layer in pdc0
			// i=1: singleLayer[I]=-1 => s=2; bothly fired layers in pdc0
			// i=1: singleLayer[I]=2: impossible, or we won't be here.
			const int s = i ? (singleLayer[I] ? 2 : 1) : 0; // offset for the subscripts
			const double dz = z[I][s+1] - z[I][s];
			if(dz < 8. || dz > 15.) // to tell if s and s+1 belong to the same PDC
				ETFMsg::Error("ETFMath", "IterFit4Single: Peculiar, supposed to be adjacent X1X2");
			const double sgn = sign(x[I][s] - x[I][s+1]); // x0-x1 within a pdc
			// X1 //
			const double R0 = r[I][s] * sgn; // the signed drift distance
			zt[I][s] = z[I][s] + R0*sinTheta[I];
			xt[I][s] = x[I][s] - R0*cosTheta[I];
			// X2 //
			const double R1 = r[I][s+1] * sgn; // the signed drift distance
			zt[I][s+1] = z[I][s+1] - R1*sinTheta[I];
			xt[I][s+1] = x[I][s+1] + R1*cosTheta[I];
		} // end for over i
	} // end for over I
	// assign signs for X1 or X2 singly fired wires //
	for(int i = 0; i < n; i++){ // each i represents a set of r signs
		// assign signs to drift distances
		for(int I = 0; I < 2; I++){ // loop over preTaTrk-postTaTrk
			if(nSingle[I]){
				const int s = singleLayer[I];
				// the signed drift distance
				const double R = r[I][s] * (2*((i>>(I*nSingle[0])) & 0x1)-1);
				zt[I][s] = z[I][s] + R*sinTheta[I];
				xt[I][s] = x[I][s] - R*cosTheta[I];
			} // end if
			LinearFit(zt[I].data(), xt[I].data(), dx[I].data(), nh[I],
				bl[I], kl[I], db, dk, d2[I], q);
		} // end for over I
		d2T = d2[0] + d2[1] + Dx2DxTa_2(kl, bl, dX);
		if(d2T < d2Tmin){ // acknowledge the result
			t0->SetK(kl[0]); t0->SetB(bl[0]); t0->SetChi2(d2[0]);
			t1->SetK(kl[1]); t1->SetB(bl[1]); t1->SetChi2(d2[1]);
			fDx2 = dX[0]; fDxTa = dX[1];
			d2Tmin = d2T;
		} // end if
	} // end for over i

	return d2Tmin;
} // end member function IterFit4Single

// the sum of squares of dx2 and dxTa, used to calculte the additional term to
// the chi2 of the ameliorated tracking
/// \param dx[0-1]: [dx2-dxTa]
double ETFMath::Dx2DxTa_2(double *k, double *b, double *dx){
	// calculate dx2 //
	double x2[2]{};
	rho(k[1], b[1], k[2], b[2], x2); // calculate rho in magField and obtain x2[2]
	dx[0] = x2[0] - x2[1];
	dx[0] -= 2.5; // an artifical shift to divert <dx2> to zero
	// calculate dxTa //
	static const double taZ = ETFDetectorManager::Instance()->GetTargetZ();
	dx[1] = (k[0] * taZ + b[0]) - (k[1] * taZ + b[1]); // dxTa
	// now let's do the weighted sum //
	// 10.*k0/0.013 = 769*k0; 3 sigma(k0) scaled to 10 mm
	double dk = fabs(k[0]-k[1]), dktmp = 200. * dk;
	if(dk > 0.02) dktmp *= 3.; // most likely a mismatch of preTaTrk and postTaTrk happened
	static const double kVdx2 = 10., kVdxTa = 2.5; // the relative variance V/sigma_DC
	return
		dx[0]*dx[0]   / kVdx2 // 10.
		+ dx[1]*dx[1] / kVdxTa // 2.5
		+ dktmp*dktmp // XXX 2020-03-12
		;
} // end member function Dx2DxTa_2

/// analytic PID method using DCTaArr
double ETFMath::rho(double k1, double b1, double k2, double b2, double *x2Arr,
	double *zo, double *xo){
	if(fabs(k1 - k2) < 1.e-8){
		ETFMsg::Warn("ETFMath", "rho: |k1-k2| too small: k1: %f, k2: %f", k1, k2);
		k2 = k1 + 1.e-7;
	} // end if

	const double z1 = kzMagIn,  x1 = k1 * z1 + b1; // incident point
	const double z2 = kzMagOut, x2 = k2 * z2 + b2; // exit point

	// x2 solved from equation L1 == L2
	const double x2p = x1+(z2-z1)/(k1+k2)*(k1*k2+sqrt(1.+k1*k1)*sqrt(1.+k2*k2)-1.);
	const double L2p = fabs(((z1-z2)+k1*(x1-x2p))/(k1-k2))*sqrt(1.+k2*k2);

	if(zo && xo){
		zo[0] = (k1*z2-k2*z1+k1*k2*(x2p-x1))/(k1-k2);
		xo[0] = (z1-zo[0])/k1+x1;
	} // end if

#ifdef DEBUG
// XXX: note that L2 is preferred, which agrees with L1p and L2p the better
	const double L1  = fabs(((z1-z2)+k2*(x1-x2 ))/(k1-k2))*sqrt(1.+k1*k1);
	const double L2  = fabs(((z1-z2)+k1*(x1-x2 ))/(k1-k2))*sqrt(1.+k2*k2);
	const double L1p = fabs(((z1-z2)+k2*(x1-x2p))/(k1-k2))*sqrt(1.+k1*k1);

	cout << "\n\nL1: " << L1 << "\tL2: " << L2; // DEBUG
	cout << "\nL1p: " << L1p << "\tL2p: " << L2p; // DEBUG
	cout << "\tx2: " << x2 << "\tx2p: " << x2p << endl; // DEBUG
	getchar(); // DEBUg
#endif

	if(x2Arr){
		x2Arr[0] = x2;
		x2Arr[1] = x2p;
	} // end if
	return L2p;
} // end member function rho

double ETFMath::BetaGamma(double beta){
	if(beta < 0. || beta >= 1.){
		ETFMsg::Error("ETFMath", "BetaGamma: input beta invalid: %f", beta);
		return -9999.;
	} // end if
	return beta/sqrt(1.-beta*beta);
} // end member function BetaGamma

/// Given a set of data x[0...ndata-1] and y[0...ndata-1] with standard deviations
/// sigy[0...ndata-1]. Fit them to a straight line y(x) = a + bx by minimizing chi2.
/// Returning a and b, and their respective probable uncertainties.siga and sigb, the
/// chi-square chi2 and the goodness-of-fit probability q (that the fit would
/// have chi2 this large or larger under the model given by the fitting). If
/// dy[0] <= 0., then the standard deviations are assumed to be unavailable: q
/// is returned as 1.0 and the normalization of chi2 is to unit standard deviation
/// on all points.
/// Ref. Numerical Recipes in C, p665.
void ETFMath::LinearFit(const double *x, const double *y, const double *dy, int ndata,
    double &a, double &b, double &siga, double &sigb, double &chi2, double &q){
  // stt=\sum_i{1/sigi*(xi-sx/s), sx=\sum_i{xi/sigi^2}, sy=\sum_i{yi/sigi^2}
  double s = 0., sx = 0., sy = 0., stt = 0.;
  bool isSigYKnown = dy[0] > 0.;
  // accumulate sums ...
  if(isSigYKnown){
    s = 0.;
    for(int i = 0; i < ndata; i++){ // ...with weights
      double wt = 1. / (dy[i]*dy[i]); // weights
      s  += wt;
      sx += wt*x[i];
      sy += wt*y[i];
    } // end for
  } // end if
  else{ // ...or without weights
    for(int i = 0; i < ndata; i++){
      sx += x[i];
      sy += y[i];
    } // end for over i
    s = ndata;
  } // end else
  double sxos = sx / s; // sx over s
  // calculate the slope b
  b = 0.;
  if(isSigYKnown){
    for(int i = 0; i < ndata; i++){
      double t = (x[i] - sxos)/dy[i];
      stt += t*t;
      b += t*y[i]/dy[i];
    } // end for
  } // end if
  else{
    for(int i = 0; i < ndata; i++){
      double t = x[i] - sxos;
      stt += t*t;
      b += t*y[i];
    } // end for over i
  } // end else

  // solve for a, b, siga and sigb
  b /= stt;
  a = (sy-sx*b)/s;
  siga = sqrt((1.+sx*sx/(s*stt))/s);
  sigb = sqrt(1./stt);

  // calculte chi2
  chi2 = 0.; q = 0.;
  double nu = ndata - 2; // degree of freedom for chi2 distribution
  if(isSigYKnown){
    for(int i = 0; i < ndata; i++) chi2 += ETFMath::sqr((y[i]-a-b*x[i])/dy[i]);
    q = ETFMath::gammaq(0.5*nu, 0.5*chi2);
  } // end if
  // for unweighted data, evaluate typical sig (which is assumed unit before)
  // using chi2, then update siga and sigb
  else{
    for(int i = 0; i < ndata; i++) chi2 += ETFMath::sqr(y[i]-a-b*x[i]);
    // chi2~nu, chi2*sig^2=\sum{{yi-y(xi)}^2}~nu*sig^2
    double sigData = sqrt(chi2 / nu);
    siga *= sigData;
    sigb *= sigData;
  } // end else

	chi2 /= 1.+b*b; // so that chi2 is sum{d^2}, where d is distance_to_line
} // end member function LinearFit

/////// functions serving 3D tracking //////
// Oxuyuzu = Rz[-theta].Ry[-phi].Oxyz: detector is along the zu axis and U wire is parallel to y axis
// Similar rotation has been implemented for V tracking
// U+V->X tranformation: lx: x=kz+b: slope
double ETFMath::kUV_X(double phi, double ku, double kv){
	return ((ku+kv)*cos(phi)+Sqrt3()*sin(phi))/(Sqrt3()*cos(phi)-(ku+kv)*sin(phi));
}
// U+V->X tranformation: lx: x=kz+b: intercept
double ETFMath::bUV_X(double phi, double ku, double kv, double bu, double bv){
	return (bu+bv)/(Sqrt3()*cos(phi)-(ku+kv)*sin(phi));
}
// U+V->Y tranformation: ly: y=kz+b: slope
double ETFMath::kUV_Y(double phi, double ku, double kv){
	return Sqrt3()*(-ku+kv)/(Sqrt3()*cos(phi)-(ku+kv)*sin(phi));
}
// U+V->Y tranformation: ly: y=kz+b: intercept
double ETFMath::bUV_Y(double phi, double ku, double kv, double bu, double bv){
	return (Sqrt3()*(-bu+bv)*cos(phi)+2.*(-bv*ku+bu*kv)*sin(phi))/(Sqrt3()*cos(phi)-(ku+kv)*sin(phi));
}
// X+Y->U tranformation: lu: xu=k zu+bu: slope
double ETFMath::kXY_U(double phi, double k1, double k2){
	return (-k2+Sqrt3()*(k1*cos(phi)-sin(phi)))/(2.*(cos(phi)+k1*sin(phi)));
}
// X+Y->U tranformation: lu: xu=k zu+bu: intercept
double ETFMath::bXY_U(double phi, double k1, double k2, double b1, double b2){
	return 0.5*(-b2+b1*(Sqrt3()+k2*sin(phi))/(cos(phi)+k1*sin(phi)));
}
// X+Y->V tranformation: lv: yv=k zv+bv: slope
double ETFMath::kXY_V(double phi, double k1, double k2){
	return (k2+Sqrt3()*(k1*cos(phi)-sin(phi)))/(2.*(cos(phi)+k1*sin(phi)));
}
// X+Y->V tranformation: lv: yv=k zv+bv: intercept
double ETFMath::bXY_V(double phi, double k1, double k2, double b1, double b2){
	return 0.5*(b2+b1*(Sqrt3()-k2*sin(phi))/(cos(phi)+k1*sin(phi)));
}
